#include "imu.hpp"

/**
 * @brief initilize
 * 
 * @param ifname 
 * @return int 
 */
int Imu::open(std::string ifname)
{
	struct sockaddr_can addr;
	int sock;
	sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
	if (sock < 0)
	{
		printf("failed to open socket can interface: %s", ifname.c_str());
		return -1;
	}
	struct ifreq ifr;
	strcpy(ifr.ifr_name, ifname.c_str());
	ioctl(sock, SIOCGIFINDEX, &ifr);
	addr.can_family = AF_CAN;
	addr.can_ifindex = ifr.ifr_ifindex;

	if (bind(sock, (struct sockaddr *)&addr, sizeof(struct sockaddr)))
	{
		printf("faild to bind socket can");
		close(sock);
		return -2;
	}

	struct timeval timeout = {0, 50000};
	setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
	timeout.tv_usec = 50000;
	setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO, (char *)&timeout, sizeof(timeout));

	printf("socket can interface %s open", ifname.c_str());
	this->_sock = sock;
	return 0;
}

Imu::Imu(int addr)
{
	this->_addr = addr;
}

Imu::~Imu()
{
	close(this->_sock);
	this->_sock = -1;
}

bool Imu::is_open()
{
	return this->_sock != -1;
}

/**
 * @brief try receive the result of a sample period, data will not be touched if failed
 * 
 * @param data the result, if successful
 * @return int 0: successful, -1: socketcan interface not open, -2: timeout
 */
int Imu::recv(imu_data_t &data)
{
	if (!this->is_open()) return -1;
	struct can_frame frame;
	bool got_accel = false, got_angula = false, got_quad = false;
	int len;
	imu_data_t out;
	do
	{
		len = read(this->_sock, &frame, sizeof(frame));
		uint16_t *p16 = (uint16_t *)frame.data;
		switch (frame.can_id - this->_addr)
		{
			case accel:
				out.accel_x = ((int16_t)le16toh(p16[0])) * 9.8 / 1000;
				out.accel_y = ((int16_t)le16toh(p16[1])) * 9.8 / 1000;
				out.accel_z = ((int16_t)le16toh(p16[2])) * 9.8 / 1000; 
				got_accel = true;
				break;
			case angula:
				out.ang_x = (int16_t)le16toh(p16[0]) * 0.1 * 3.14159 / 180;
				out.ang_y = (int16_t)le16toh(p16[1]) * 0.1 * 3.14159 / 180;
				out.ang_z = (int16_t)le16toh(p16[2]) * 0.1 * 3.14159 / 180;
				got_angula = true;
				break;
			case euler:
				break;
			case quad:
				out.quad_x = (int16_t)le16toh(p16[0]) / 10000.0;
				out.quad_y = (int16_t)le16toh(p16[1]) / 10000.0;
				out.quad_z = (int16_t)le16toh(p16[2]) / 10000.0;
				out.quad_w = (int16_t)le16toh(p16[3]) / 10000.0;
				got_quad = true;
				break;
			case pascal:
				break;
		}
		if (got_accel && got_angula && got_quad)
		{
			// got all we need
			data = out;
			return 0;
		}
	} while (len > 0);
	// timeout, return
	return -2;
}
